﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Diagnostics.Contracts;

namespace PhysicsEngine.Common
{
  /// <summary>
  /// A vector implementation. Based loosely on the Vector2 implementation in the XNA framework Incorporates Code Contracts for specification purposes.
  /// </summary>
  public class Vector2 : IEquatable<Vector2>, IDisposable
  {
    public float X { get; set; }
    public float Y { get; set; }
    public float LengthSquared { get; private set; }

    private static Queue<Vector2> _vectorPool = new Queue<Vector2>();

    public static Vector2 Acquire(float x, float y)
    {
      Contract.Ensures(Contract.Result<Vector2>().X == x);
      Contract.Ensures(Contract.Result<Vector2>().Y == y);

      if (_vectorPool.Count == 0)
      { 
        return new Vector2(x, y); 
      }

      Vector2 returnVector = _vectorPool.Dequeue();

      returnVector.X = x;
      returnVector.Y = y;

      return returnVector;
    }

    /// <summary>
    /// Public constructor for a Vector2 object.
    /// </summary>
    /// <param name="X">The X component of the vector.</param>
    /// <param name="Y">The Y component of the vector.</param>
    private Vector2(float x, float y)
    {
      Contract.Ensures(this.X == x);
      Contract.Ensures(this.Y == y);

      X = x;
      Y = y;

      LengthSquared = (X * X) + (Y * Y);
    }

    [ContractInvariantMethod]
    private void ObjectInvariant () {
      Contract.Invariant(LengthSquared == (X * X) + (Y * Y));
      //Contract.Invariant(_vectorPool != null);
      //Contract.ForAll<Vector2>(_vectorPool, item => item != null);
    }

    /// <summary>
    /// Gets the length of the vector.
    /// </summary>
    public float Length
    {
      [Pure]
      get
      {
        Contract.Ensures(Contract.Result<float>() == (float)Math.Sqrt(LengthSquared));

        return (float)Math.Sqrt(LengthSquared);
      }
    }

    /// <summary>
    /// Compares two Vector2 objects for equality.
    /// </summary>
    /// <remarks>
    /// Two Vector2 objects are equal if their X values are equal and their Y values are equal.
    /// </remarks>
    /// <param name="v1">The first vector to compare.</param>
    /// <param name="v2">The second vector to compare.</param>
    /// <returns>True if the vectors are value equal, false otherwise.</returns>
    [Pure]
    public static bool operator ==(Vector2 v1, Vector2 v2)
    {
      Contract.Ensures(v1.Equals(v2) || !Contract.Result<bool>());

      return v1.Equals(v2);
    }

    /// <summary>
    /// Compares two Vector2 objects for inequality.
    /// </summary>
    /// <param name="v1">The first vector to compare.</param>
    /// <param name="v2">The second vector to compare.</param>
    /// <returns>True if the vectors are not equal, false otherwise.</returns>
    [Pure]
    public static bool operator !=(Vector2 v1, Vector2 v2)
    {
      Contract.Ensures((!(v1 == v2)) || !Contract.Result<bool>());

      return !(v1 == v2);
    }

    /// <summary>
    /// Allows division of a Vector2 by a float.
    /// </summary>
    /// <param name="v1">The vector to divide.</param>
    /// <param name="scalar">The scalar to divide by.</param>
    /// <returns>A new Vector2 object with X and Y components equal to the input X and Y divided by the input float.</returns>
    [Pure]
    public static Vector2 operator /(Vector2 v1, float scalar)
    {
      Contract.Ensures(Contract.Result<Vector2>().X == (v1.X / scalar));
      Contract.Ensures(Contract.Result<Vector2>().Y == (v1.Y / scalar));

      float divX = v1.X / scalar;
      float divY = v1.Y / scalar;

      return Acquire(divX, divY);
    }

    /// <summary>
    /// Allows multiplication of a Vector2 by a float.
    /// </summary>
    /// <param name="v1">The vector to multiply.</param>
    /// <param name="scalar">The scalar to multiply by.</param>
    /// <returns>A new Vector2 object with X and Y components equal to the input X and Y multiplied by the input float.</returns>
    [Pure]
    public static Vector2 operator *(Vector2 v1, float scalar)
    {
      Contract.Ensures(Contract.Result<Vector2>().X == v1.X * scalar);
      Contract.Ensures(Contract.Result<Vector2>().Y == v1.Y * scalar);

      float multX = v1.X * scalar;
      float multY = v1.Y * scalar;

      return Acquire(multX, multY);
    }

    /// <summary>
    /// Allows addition of two Vector2 values.
    /// </summary>
    /// <param name="v1">The first vector to add.</param>
    /// <param name="v2">The second vector to add.</param>
    /// <returns>A new vector that is the result of the addition.</returns>
    [Pure]
    public static Vector2 operator +(Vector2 v1, Vector2 v2)
    {
      Contract.Ensures(Contract.Result<Vector2>().X == v1.X + v2.X);
      Contract.Ensures(Contract.Result<Vector2>().Y == v1.Y + v2.Y);

      float addX = v1.X + v2.X;
      float addY = v1.Y + v2.Y;

      return Acquire(addX, addY);
    }
    
    /// <summary>
    /// Allows subtraction of two Vector2 values.
    /// </summary>
    /// <param name="v1">The first vector in the subtraction.</param>
    /// <param name="v2">The second vector in the subtraction.</param>
    /// <returns>A new vector that is the result of the subtraction.</returns>
    [Pure]
    public static Vector2 operator -(Vector2 v1, Vector2 v2)
    {
      Contract.Ensures(Contract.Result<Vector2>().X == v1.X - v2.X);
      Contract.Ensures(Contract.Result<Vector2>().Y == v1.Y - v2.Y);

      float diffX = v1.X - v2.X;
      float diffY = v1.Y - v2.Y;

      return Acquire(diffX, diffY);
    }

    /// <summary>
    /// Normalizes the vector. This is achieved by scaling the vector down by a factor equal to its length.
    /// </summary>
    /// <returns>A unit vector with a length equal to 1.</returns>
    [Pure]
    public Vector2 Normalize()
    {
      Contract.Ensures(Contract.Result<Vector2>().X == X / Length);
      Contract.Ensures(Contract.Result<Vector2>().Y == Y / Length);

      Vector2 unitVector = this / Length;

      // asking the analyzer to prove this would be difficult, so check it at runtime
      Contract.Assume(unitVector.Length == 1);

      return unitVector;
    }

    /// <summary>
    /// Produces the dot product of the given vectors.
    /// </summary>
    /// <param name="v1">The first vector from which to form a dot product.</param>
    /// <param name="v2">The second vector from which to form a dot product.</param>
    /// <returns>The dot product of the given vectors.</returns>
    [Pure]
    public static float DotProduct(Vector2 v1, Vector2 v2)
    {
      Contract.Ensures(Contract.Result<float>() == (v1.X * v2.X) + (v1.Y * v2.Y));

      return (v1.X * v2.X) + (v1.Y * v2.Y);
    }

    /// <summary>
    /// Delivers the cross product of two vectors.
    /// </summary>
    /// <remarks>
    /// This cross product method gives the length of the Z axis vector that should be the result of a 3D cross product operation. It is useful in certain circumstances.
    /// </remarks>
    /// <param name="v1">The first vector from which to derive the cross product.</param>
    /// <param name="v2">The second vector from which to derive the cross product.</param>
    /// <returns>A scalar value representing the length of the Z component vector which would have been created in the 3D vector cross product.</returns>
    [Pure]
    public static float CrossProduct(Vector2 v1,  Vector2 v2)
    {
      Contract.Ensures(Contract.Result<float>() == (v1.X * v2.Y) - (v1.Y * v2.X));
      
      return (v1.X * v2.Y) - (v1.Y * v2.X);
    }

    /// <summary>
    /// Delivers the cross product of a vector and the Z vector with magnitude equal to the scalar.
    /// </summary>
    /// <remarks>
    /// This cross product method essentially gives a +90 degree (perpendicular) vector relative to the input vector and scaled by a factor of the scalar value.
    /// </remarks>
    /// <param name="v1">The input vector.</param>
    /// <param name="scalar">The magnitude of the Z vector.</param>
    /// <returns>A vector perpendicular (rotated counter clockwise) to the input vector with magnitude increased by a factor of the scalar value.</returns>
    [Pure]
    public static Vector2 CrossProduct(Vector2 v1, float scalar)
    {
      Contract.Ensures(Contract.Result<Vector2>().X == v1.Y * scalar);
      Contract.Ensures(Contract.Result<Vector2>().Y == v1.X * (-scalar));

      return Acquire(v1.Y * scalar, v1.X * (-scalar));
    }

    /// <summary>
    /// Delivers the cross product of a vector and the Z vector with magnitude equal to the scalar.
    /// </summary>
    /// <remarks>
    /// This cross product method essentially gives a -90 degree (perpendicular) vector relative to the input vector and scaled by a factor of the scalar value.
    /// </remarks>
    /// <param name="v1">The input vector.</param>
    /// <param name="scalar">The magnitude of the Z vector.</param>
    /// <returns>A vector perpendicular (rotated clockwise) to the input vector with magnitude increased by a factor of the scalar value.</returns>
    [Pure]
    public static Vector2 CrossProduct(float scalar, Vector2 v1)
    {
      Contract.Ensures(Contract.Result<Vector2>().X == v1.Y * (-scalar));
      Contract.Ensures(Contract.Result<Vector2>().Y == v1.X * scalar);

      return Acquire(v1.Y * (-scalar), v1.X * scalar);
    }

    [Pure]
    public static Vector2 Cast(object obj)
    {
      Contract.Requires<InvalidCastException>(obj != null);
      Contract.Requires<InvalidCastException>(obj is Vector2);

      return (Vector2)obj;
    }

    [Pure]
    public override bool Equals(object obj)
    {
      Contract.Ensures((obj != null || obj is Vector2) || !Contract.Result<bool>());
      Contract.Ensures(!(obj is Vector2) || (Contract.Result<bool>() == this.Equals(Cast(obj))));

      if (!(obj != null || obj is Vector2))
      {
        return false;
      }

      Contract.Assume(obj is Vector2);

      return this.Equals(Cast(obj));
    }

    public override int GetHashCode()
    {
      unchecked // Overflow is fine, just wrap
      {
        int hash = 17;
        hash = hash * 23 + X.GetHashCode();
        hash = hash * 23 + Y.GetHashCode();
        return hash;
      }
    }

    public override string ToString()
    {
      return "[" + X.ToString() + ", " + Y.ToString() + "]";
    }

    [Pure]
    public bool Equals(Vector2 other)
    {
      Contract.Ensures(other != null || !Contract.Result<bool>());
      Contract.Ensures(((this.X == other.X) && (this.Y == other.Y)) || !Contract.Result<bool>());

      if (other == null)
      {
        return false;
      }

      return ((this.X == other.X) && (this.Y == other.Y));
    }

    private void updateLen()
    {
      Contract.Ensures(this.X == Contract.OldValue<float>(X));
      Contract.Ensures(this.Y == Contract.OldValue<float>(Y));
 	    Contract.Ensures(this.LengthSquared == (X * X) + (Y * Y));

      this.LengthSquared = (X * X) + (Y * Y);
    }


    public void Dispose()
    {
      _vectorPool.Enqueue(this);
    }
  }
}